#include "pid.h"

#include "time_utils.h"
#include "foc_utils.h"

PIDController::PIDController(float p, float i, float d, float ramp, float limit) :
        p_(p), i_(i), d_(d), output_ramp_(ramp), limit_(limit), error_prev_(0.0f), output_prev_(0.0f), integral_prev_(0.0f)
{
    timestamp_prev_ = micros();
}

PIDController::~PIDController()
{

}

// PID controller function
float PIDController::operator()(float error)
{
    // calculate the time from the last call
    unsigned long timestamp_now = micros();
    float Ts = (timestamp_now - timestamp_prev_) * 1e-6f;
    if (Ts <= 0 || Ts > 0.5f)
        Ts = 1e-3f;

    // u(s) = (P + I/s + Ds)e(s)
    // Discrete implementations
    // proportional part
    // u_p  = P *e(k)
    float proportional = p_ * error;
    // Tustin transform of the integral part
    // u_ik = u_ik_1  + I*Ts/2*(ek + ek_1)
    float integral = integral_prev_ + i_ * Ts * 0.5f * (error + error_prev_);
    // antiwindup - limit the output
    integral = _constrain(integral, -limit_, limit_);
    // Discrete derivation
    // u_dk = D(ek - ek_1)/Ts
    float derivative = d_ * (error - error_prev_) / Ts;

    // sum all the components
    float output = proportional + integral + derivative;
    // antiwindup - limit the output variable
    output = _constrain(output, -limit_, limit_);

    // if output ramp defined
    if (output_ramp_ > 0)
    {
        // limit the acceleration by ramping the output
        float output_rate = (output - output_prev_) / Ts;
        if (output_rate > output_ramp_)
            output = output_prev_ + output_ramp_ * Ts;
        else if (output_rate < -output_ramp_)
            output = output_prev_ - output_ramp_ * Ts;
    }
    // saving for the next pass
    integral_prev_ = integral;
    output_prev_ = output;
    error_prev_ = error;
    timestamp_prev_ = timestamp_now;

    return output;
}
